# -*- coding: utf-8 -*-
import numpy as np
import matplotlib.pyplot as plt
import copy

class BPNN:
    def __init__(self,wi,wo):
        self.IN=4
        self.H=5
        self.Out=3
        self.wi = np.mat('-0.2846    0.2193   -0.5097   -1.0668;-0.7484   -0.1210   -0.4708    0.0988;-0.7176    0.8297   -1.6000    0.2049;-0.0858    0.1925   -0.6346    0.0347;0.4358    0.2369   -0.4564   -0.1324');
        self.wo = np.mat('1.0438    0.5478    0.8682    0.1446    0.1537;0.1716    0.5811    1.1214    0.5067    0.7370;1.0063    0.7428    1.0534    0.7824    0.6494');
        self.last_wo = copy.deepcopy(self.wo)
        self.last_last_wo = copy.deepcopy(self.wo)
        self.last_wi = copy.deepcopy(self.wi)
        self.last_last_wi = copy.deepcopy(self.wi)
        self.last_error = 0
        self.last_last_error = 0
        self.last_u = 0
        self.last_du = 0
        self.last_yout = 0
        self.xite = 0.2
        self.alfa = 0.05
        self.last_yout = 0
        self.count = 0
    def train(self):
        pass
    def sim(self,rin,yout,error):
        xi=[rin,yout,error,1]
        epid=[(error-self.last_error),error,(error-2*self.last_error+self.last_last_error)]
        self.I=(xi*(self.wi.T))
        self.Oh =np.mat('0.0 ;0.0 ;0.0 ;0.0 ;0.0')
        for j in range(self.H):
            self.Oh[j,0]=(np.exp(self.I[0,j])-np.exp(-self.I[0,j]))/(np.exp(self.I[0,j])+np.exp(-self.I[0,j])) #Middle Layer
        self.K =self.wo*(self.Oh)             #Output Layer
        for l in range(self.Out):
            self.K[l]=np.exp(self.K[l])/(np.exp(self.K[l])+np.exp(-self.K[l]))        #Getting kp,ki,kd sigmoid 限幅
        # print "K[%d]=%f"%(l,K[l])
        self.kp=self.K[0,0]
        self.ki=self.K[1,0]
        self.kd=self.K[2,0]
        Kpid=[self.kp,self.ki,self.kd]
        #print self.count,Kpid
        self.count += 1
        du=np.dot(Kpid,epid)  #pid的增量值
        u=self.last_u+du #pid的输出值

        dyu = np.sign((yout-self.last_yout)/(du-self.last_du+0.0001))  #y对du的符号函数用于替代y对du的偏导数
        dK = np.zeros(self.Out)
        for j in range(self.Out):
            dK[j] = (2/((np.exp(self.K[j])+np.exp(-self.K[j]))**2))
        delta3 = np.zeros(self.Out)
        for l in range(self.Out):
            delta3[l] = (error*dyu*epid[l]*dK[l])

        d_wo = 0.0
        for l in range(self.Out):
            for i in range(self.H):
                d_wo=self.xite*delta3[l]*self.Oh[i,0]
                d_wo = np.add(np.dot(self.alfa,(self.last_wo-self.last_last_wo)),d_wo)

        self.wo=copy.deepcopy(self.last_wo+d_wo+self.alfa*(self.last_wo-self.last_last_wo))#输出层权值更新
        #Hidden layer
        dO = np.zeros(self.H)
        for i in range(self.H):
            dO[i] = (4/((np.exp(self.I[0,i])+np.exp(-self.I[0,i]))**2))

        segma=delta3*self.wo
        delta2 = np.zeros(self.H)
        for i in range(self.H):
            delta2[i] = (dO[i]*segma[0,i])
        d_wi=np.dot(self.xite,delta2)
        d_wi = np.mat(d_wi).T*np.mat(xi)
        self.wi=copy.deepcopy(self.last_wi+d_wi+np.dot(self.alfa,(self.last_wi-self.last_last_wi)))

        #Parameters Update
        self.last_u=u
        self.last_yout = yout
        self.last_du = du
        self.last_last_wo=copy.deepcopy(self.last_wo)
        self.last_wo=copy.deepcopy(self.wo)
        self.last_last_wi=copy.deepcopy(self.last_wi)
        self.last_wi=copy.deepcopy(self.wi)


        self.last_last_error=self.last_error
        self.last_error=error
        return (self.kp,self.ki,self.kd,self.wo,self.wi,u)
